#include "Triangulator.hpp"
#include "../VisionTools.hpp"
#include <zMat.hpp>

namespace zzz{
Vector3d Triangulator::LinearTriangulate(const vector<ProjectionMat<double> > &P, const vector<Vector2d> &pos2ds)
{
  zuint n=P.size();
  zMatrix<double> A(n*2,4),U,S,VT;
  
  for (zuint i=0; i<n; i++)
  {
    A(i*2,  Colon()) = Trans(Dress(P[i].P().Row(2)*pos2ds[i][0]-P[i].P().Row(0)));
    A(i*2+1,Colon()) = Trans(Dress(P[i].P().Row(2)*pos2ds[i][1]-P[i].P().Row(1)));
  }

  bool svdres=SVD(A,U,S,VT);
  ZCHECK(svdres)<<"SVD failed in linear triangulate.";
  Vector4d X(VT(3,0),VT(3,1),VT(3,2),VT(3,3));
  return FromHomogeneous(X);
}

void Triangulator::LinearTriangulate(const ProjectionMatd P[2], const vector<Vector2d> pos2ds[2], vector<Vector3d> &pos3ds)
{
  zuint n=pos2ds[0].size();
  pos3ds.clear();
  pos3ds.reserve(n);
  zMatrix<double> A(4,4),U,S,VT;
  Vector4d X;

  for (zuint i=0; i<n; i++)
  {
    A(0,Colon()) = Trans(Dress(P[0].P().Row(2)*pos2ds[0][i][0]-P[0].P().Row(0)));
    A(1,Colon()) = Trans(Dress(P[0].P().Row(2)*pos2ds[0][i][1]-P[0].P().Row(1)));
    A(2,Colon()) = Trans(Dress(P[1].P().Row(2)*pos2ds[1][i][0]-P[1].P().Row(0)));
    A(3,Colon()) = Trans(Dress(P[1].P().Row(2)*pos2ds[1][i][1]-P[1].P().Row(1)));

    bool svdres=SVD(A,U,S,VT);
    ZCHECK(svdres)<<"SVD failed is linear triangulate";
    Dress(X)=Trans(VT(3,Colon()));
    pos3ds.push_back(FromHomogeneous(X));
  }
}

void Triangulator::LinearTriangulate(SceneData<double> &sd)
{
  for (zuint p=0; p<sd.points_.size(); p++)
  {
    const vector<SceneData<double>::Point2D> &pos2ds=sd.points_[p].pos2ds;
    zuint n=pos2ds.size();
    zMatrix<double> A(n*2,4),U,S,VT;

    for (zuint i=0; i<n; i++)
    {
      const ProjectionMatd &P=sd.cameras_[pos2ds[i].img_id].camera;
      A(i*2,  Colon()) = Trans(Dress(P.P().Row(2)*pos2ds[i].pos2d[0]-P.P().Row(0)));
      A(i*2+1,Colon()) = Trans(Dress(P.P().Row(2)*pos2ds[i].pos2d[1]-P.P().Row(1)));
    }

    bool svdres=SVD(A,U,S,VT);
    ZCHECK(svdres)<<"SVD failed in linear triangulate";
    Vector4d X(VT(3,0),VT(3,1),VT(3,2),VT(3,3));
    sd.points_[p].pos3d=FromHomogeneous(X);
  }
}

void Triangulator::NonLinearTriangulate(const ProjectionMatd P[2], const vector<Vector2d> pos2ds[2], vector<Vector3d> &pos3ds)
{

}

}
